﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace AutoWelding.Utils
{
    /// <summary>
    /// 机器人工具类
    /// </summary>
    public class RobotUtils
    {
        #region 机器人拾取料盘产品

        /// <summary>
        /// 查找指定料盘上第一个有产品的位置
        /// </summary>
        /// <param name="trayId">料盘ID</param>
        /// <returns>产品坐标，如果没有产品则返回null</returns>
        public static string FindFirstProduct(int trayId)
        {
            // 查找指定料盘上的产品
            // 产品坐标从(1,1)到(4,4)，总共16个坐标点

            for (int row = 1; row <= 4; row++)
            {
                for (int col = 1; col <= 4; col++)
                {
                    // 检查该位置是否有产品
                    if (HasProductAtPosition())
                    {
                        // 返回找到的产品坐标
                        return $"{row}-{col}";
                    }
                }
            }

            // 没有找到产品
            return null;
        }

        /// <summary>
        /// 检查指定位置是否有产品
        /// </summary>
        /// <returns></returns>
        /// <exception cref="NotImplementedException"></exception>
        private static bool HasProductAtPosition()
        {
            return true;
        }

        /// <summary>
        /// 拾取指定坐标的产品
        /// </summary>
        /// <param name="trayId">料盘ID</param>
        /// <param name="position">坐标字符串，格式为"行-列"</param>
        /// <returns>是否成功拾取</returns>
        public static bool PickProductAtPosition(int trayId, string position)
        {
            if (string.IsNullOrEmpty(position))
                return false;

            try
            {
                // 解析坐标
                string[] parts = position.Split('-');
                if (parts.Length == 2 &&
                    int.TryParse(parts[0], out int row) &&
                    int.TryParse(parts[1], out int col))
                {
                    // 机器人移动到指定坐标
                    MoveRobotToPosition(row, col);

                    // 机器人拾取产品
                    RobotPickProduct();

                    Console.WriteLine($"成功从料盘{trayId}的位置({row},{col})拾取产品");
                    return true;
                }
            }
            catch (Exception ex)
            {
                Console.WriteLine($"拾取产品时发生错误: {ex.Message}");
            }

            return false;
        }
        /// <summary>
        /// 机器人拾取产品
        /// </summary>
        /// <exception cref="NotImplementedException"></exception>
        private static void RobotPickProduct()
        {
            //夹爪抓取产品
            GripperUtils.RobotGripperClose();
        }

        /// <summary>
        /// 移动机器人到指定坐标
        /// </summary>
        /// <param name="row"></param>
        /// <param name="col"></param>
        /// <exception cref="NotImplementedException"></exception>
        private static void MoveRobotToPosition(int row, int col)
        {
            //调用通讯模块中的方法
        }

        /// <summary>
        /// 机器人把产品放到转接台一号位
        /// </summary>
        public static void RobotMoveToPlace_01()
        {
            //调用通讯模块中的方法
        }

        /// <summary>
        /// 机器人把产品放到转接台二号位
        /// </summary>
        public static void RobotMoveToPlace_02()
        {
            //调用通讯模块中的方法
        }

        #endregion
    }
}
